/************************************************
 *** Datei: ARDroneV1.cpp
 ***
 *** Erstellt am: 16.05.2012
 ***       Autor: Sebastian Protz <sebastian.protz@googlemail.com>
 ***	   Version: 1.0
 ***	
 ************************************************/

#include "ARDroneV1.h"


ARDroneV1::ARDroneV1()  {
	this->droneModelName = string("Parrot ARDroneV1");
	//Command
	this->connected = false;
	this->commandCounter = 0;
	this->commandBuffer = (char*) malloc(ARDRONE_BUFFER_SIZE);
	memset(this->commandBuffer, 0, ARDRONE_BUFFER_SIZE);
	this->commandBufferSaturation = 0;

	//NavData
	this->navData = new ARDroneNavData();
	this->navDataBufferSaturation = 0;
	this->navDataBuffer = (char*) malloc(ARDRONE_NAVDATA_BUFFER_SIZE);
	this->navDataReadPointer = navDataBuffer;
	memset(this->navDataBuffer, 0, ARDRONE_NAVDATA_BUFFER_SIZE);
	this->checkSumID = 0;
	this->checkSumSize = 0;
	this->checkSumData = 0;
	this->navData_droneStateConfig = false;

	// Standard-Eingabegeraet
	this->inputDeviceList = new vector<IDroneController*>();
	Keyboard* kb = new Keyboard();
	this->inputDeviceList->push_back(kb);
	this->droneController = this->inputDeviceList->at(0);
	kb->run();
}


/**
 * Drone Model Name
 */
string ARDroneV1::getDroneModelName() {
	return this->droneModelName;
}



/*
 * Sets IDroneController
 */
void ARDroneV1::setDroneController(IDroneController* droneController) {
	if(droneController==NULL) {
		this->droneController = this->inputDeviceList->at(0);
	} else {
		this->droneController = droneController;
	}
}
/*
 * Gets IDroneController of Drone
 */
IDroneController* ARDroneV1::getDroneController() {
	return this->droneController;
}

void ARDroneV1::addDroneController(IDroneController* droneController) {
	//TODO
}

vector<IDroneController*>* ARDroneV1::getInputDeviceList() {
	return this->inputDeviceList;
}

IDroneNavData* ARDroneV1::getNavData() const {
	return this->navData;
}

IDroneVideoData* ARDroneV1::getVideoData() const {
	return NULL;
}



/******************************************************************************************
 * 									COMMANDS											  *
 *****************************************************************************************/


/*
 * Connects to Drone and sends initilization commands
 *
 */
void ARDroneV1::connectDrone() {
	// COMMAND
	//unknown Parrot Initialization Commands
	miscellaneousCommand();
	pModeCommand(2);
	resetNavMask();
	// connect...
	//printf("connect Drone.. \n");
	this->commandThread_active = true;
	this->commandThread = new thread(&ARDroneV1::run, this);
	connected = true;

	//NAVIGATION DATA
	this->initNavDataStream();
	this->navDataThread_active = true;
	this->navDataThread = new thread(&ARDroneV1::receiveNavigationData, this);
}

/**
 * Builds and sends commands to drone
 */
void ARDroneV1::run() {

	io_service 		ioService;
	udp::socket 	socket(ioService);
	udp::endpoint 	destEndpoint(address_v4::from_string(ARDRONE_IP), ARDRONE_CONTROL_PORT);

	try {
		socket.open(udp::v4());
		socket.set_option(udp::socket::reuse_address(true));

	} catch (std::exception& e) {
		cerr << "error socket.open: " << e.what() << endl;
	}
	while(this->commandThread_active) {

		//build Command
		resetWatchDog();
		buildNextMovementCommandToSend();
		if (this->navData_droneStateConfig) {
			this->setNavigationOption();
			this->navData_droneStateConfig = false;
		}

		//this->takeOff();
		//cout << commandBuffer << endl;

		try{
			socket.send_to(buffer(commandBuffer, commandBufferSaturation), destEndpoint );
		} catch (std::exception& e) {
			cerr << "error socket.send_to: " << e.what() << endl;
		}

		// Reset Command Buffer
		memset(this->commandBuffer, 0, ARDRONE_BUFFER_SIZE);
		this->commandBufferSaturation = 0;
		// Wait
		this_thread::sleep(boost::posix_time::milliseconds(ARDRONE_SEND_INTERVAL));
	}
	try {
		socket.close();
	} catch (std::exception& e) {
		cerr << "error socket.close: " << e.what() << endl;
	}
}



/**
 * Fills buffer with commands for drone according to received command from controller
 * deletes command
 */
void ARDroneV1::buildNextMovementCommandToSend() {
	if (this->droneController != NULL) {
		DroneCommand* command = this->droneController->getNextDroneCommand();
		switch (command->getSpecialCommand()) {
			case DRONE_SPECIAL_COMMAND_EMERGENCY :
				printf("DRONE received EMERGENCY\n");
				emergency();
				break;
			case DRONE_SPECIAL_COMMAND_START :
				printf("DRONE received START\n");
				takeOff();
				break;
			case DRONE_SPECIAL_COMMAND_LAND :
				printf("DRONE received LAND\n");
				land();
				break;
			case DRONE_SPECIAL_COMMAND_HOVER :
				this->resetWatchDog();
				break;
			case DRONE_SPECIAL_COMMAND_NONE :
				//printf("DRONE received MOVE: roll: %4.3f pitch: %4.3f yaw: %4.3f vert: %4.3f \n", command->getRoll(), command->getPitch(), command->getYaw(), command->getVertical() );
				move(command->getRoll(), command->getPitch(), command->getYaw(), command->getVertical());
				break;
			}
		delete command;
	} else {
		//TODO
		printf("NO CONTROLLER ATTACHED TO DRONE");
	}

}

void ARDroneV1::disconnectDrone() {
	// TODO
	// Deactivate Thread
	this->commandThread_active = false;
	this->commandThread->join();
	this->connected = false;

}

bool ARDroneV1::isConnected() {
	return this->connected;
}

/**
 * Resets command for Drone WatchDog
 */
void ARDroneV1::resetWatchDog() {
	this->commandBufferSaturation += sprintf(this->commandBuffer + this->commandBufferSaturation, "AT*COMWDG=%d\r", ++this->commandCounter);
}

/**
 * Unknown Parrot Command, needs to be send by connecting to Drone
 */
void ARDroneV1::miscellaneousCommand() {
	this->commandBufferSaturation += sprintf(this->commandBuffer + this->commandBufferSaturation, "AT*MISC=%d,%d,%d,%d,%d\r", ++this->commandCounter, 2, 20, 2000, 3000);
}

/**
 * Unknown Parrot Mode Command
 * @param mode Crontrol Mode
 */
void ARDroneV1::pModeCommand(int32 mode) {
	this->commandBufferSaturation += sprintf(this->commandBuffer + this->commandBufferSaturation, "AT*PMODE=%d,%d\r", ++this->commandCounter, mode);
}

/**
 * Resets Navigation Mask of Drone to enable Navigation Data sending
 */
void ARDroneV1::resetNavMask() {
	this->commandBufferSaturation += sprintf(this->commandBuffer + this->commandBufferSaturation, "AT*CTRL=%d,%d\r", ++this->commandCounter, 5);
}

/**
 * Sets the internal horizon sensors to the current position of the
 * Drone. Should be used for calibrating the sensors.
 */
void ARDroneV1::fTrim() {
	this->commandBufferSaturation += sprintf(this->commandBuffer + this->commandBufferSaturation, "AT*FTRIM=%d\r", ++this->commandCounter);
	}

void ARDroneV1::setNavigationOption() {
	this->commandBufferSaturation += sprintf(this->commandBuffer + this->commandBufferSaturation, "AT*CONFIG=%d,%s,%s\r", ++this->commandCounter, "\"general:navdata_demo\"","\"TRUE\"");
}


/**
 * Forces Drone to stop immediately
 */
void ARDroneV1::emergency() {
		this->commandBufferSaturation += sprintf(this->commandBuffer + this->commandBufferSaturation, "AT*REF=%d,%d\r", ++this->commandCounter, 256);
}

/**
 * Take Off for the Drone
 */
void ARDroneV1::takeOff() {
	this->commandBufferSaturation += sprintf(this->commandBuffer + this->commandBufferSaturation, "AT*REF=%d,%d\r", ++this->commandCounter, 290718208);
}

/**
 * Land the Drone
 */
void ARDroneV1::land() {
	this->commandBufferSaturation += sprintf(this->commandBuffer + this->commandBufferSaturation, "AT*REF=%d,%d\r", ++this->commandCounter, 290717696);
}

/**
 * 	Moving the Drone
 */
void ARDroneV1::move(float roll, float pitch, float yaw, float vertical) {
	this->commandBufferSaturation += sprintf(this->commandBuffer + this->commandBufferSaturation, "AT*PCMD=%d,%d,%d,%d,%d,%d\r",
			++this->commandCounter,  (int32)1,  (int32)roll, (int32)pitch, (int32)vertical, (int32)yaw);
}


/**
 * Plays an animation of a predetermined sequence of movements.
 * @param animation: integer value representing the animtaion style.
 * @param duration: duration in seconds.
 */
void ARDroneV1::animation(int32 animation, int32 duration) {
	this->commandBufferSaturation += sprintf(this->commandBuffer + this->commandBufferSaturation, "AT*ANIM=%d,%d,%d\r", ++this->commandCounter, animation, duration);
}


/******************************************************************************************
 * 									NAVDATA												  *
 *****************************************************************************************/


/*
 * Send few Bytes to Navigation Port to start Navigation Stream
 */
void ARDroneV1::initNavDataStream() {
		io_service 		ioService;
		udp::socket 	socket(ioService);
		udp::endpoint 	destEndpoint(address_v4::from_string(ARDRONE_IP), ARDRONE_NAVIGATION_PORT);
		udp::endpoint 	localEndpoint(address_v4::any(), ARDRONE_NAVIGATION_PORT );

		this_thread::sleep(boost::posix_time::milliseconds(ARDRONE_SEND_INTERVAL * 100));
		try {
			socket.open(udp::v4());
			socket.set_option(udp::socket::reuse_address(true));
			socket.bind(localEndpoint);

		} catch (std::exception& e) {
			cerr << "error socket.open: " << e.what() << endl;
		}

		try{
			// choose Unicast navigation sending to lower network traffic
			socket.send_to(buffer(ARDRONE_NAVIGATION_USE_UNICAST, 4), destEndpoint );
		} catch (std::exception& e) {
			cerr << "error socket.send_to: " << e.what() << endl;
		}

		try {
			socket.close();
		} catch (std::exception& e) {
			cerr << "error socket.close: " << e.what() << endl;
		}

}


/*
 * Receives and decodes Navigation Data from Drone
 */
void ARDroneV1::receiveNavigationData() {
		io_service 		ioService;
		udp::socket 	socket(ioService);
		udp::endpoint	senderEndpoint;
		udp::endpoint 	localEndpoint(address_v4::any(), ARDRONE_NAVIGATION_PORT );

		try {
			socket.open(udp::v4());
			socket.set_option(udp::socket::reuse_address(true));
			socket.bind(localEndpoint);

		} catch (std::exception& e) {
			cerr << "error socket.open: " << e.what() << endl;
		}

		while(this->navDataThread_active) {
			// receive
			try{
				this->navDataBufferSaturation = socket.receive_from( buffer(this->navDataBuffer, ARDRONE_NAVDATA_BUFFER_SIZE), senderEndpoint);
			} catch (std::exception& e) {
				cerr << "error socket.receive_from: " << e.what() << endl;
			}
			// decode
			//printf("Start decode");
			this->decodeReceivedPackage();
			this->checkDroneState();
			this->navDataBufferSaturation = 0;
			this->navDataReadPointer = this->navDataBuffer;
			//printf("NavData: roll: %4.3f pitch: %4.3f yaw: %4.3f vert: %d \n", this->navData->getRoll(), this->navData->getPitch(), this->navData->getYaw(), this->navData->getHeight());
		}

		socket.close();

}

/*
 * Reads Navigation Data Buffer and decodes Information
 */
void ARDroneV1::decodeReceivedPackage() {
	if(this->readNext4Bytes() != ARDRONE_NAVIGATION_HEADER) {
		printf("Navigation data package lost due incorrect header.");
	} else {

		this->navData->setDroneState(this->readNext4Bytes());
		this->navData->setSequenceNumber(this->readNext4Bytes());
		this->navData->setVisionFlag(this->readNext4Bytes());
		bool decOpt = false;
		do {
			int32 offset = this->navDataReadPointer - this->navDataBuffer;
			decOpt = offset < ((this->navDataBufferSaturation - ARDRONE_OPTION_SIZE));
			if (decOpt) {
				this->decodeOption();
			}
		} while (decOpt) ;

		this->checkSumID = this->readNext2Bytes();
		this->checkSumSize = this->readNext2Bytes();
		this->checkSumData = this->readNext4Bytes();
	}
}

/*
 * Decodes Option from Buffer and invokes corresponding method
 */
void ARDroneV1::decodeOption() {
	int16 optionID = this->readNext2Bytes();
	int16 optionSize = this->readNext2Bytes();
	if (optionSize != 0) {
		switch(optionID) {
			case 0:
				this->optionDemoTag(optionSize); break;
			case 1:
				printf("%s\n", "Time tag found. Not yet implemented!");
				break;
			case 2:
				printf("%s\n", "Raw measures tag found. Not yet implemented!");
				break;
			case 3:
				printf("%s\n", "Physical measures tag found. Not yet implemented!");
				break;
			case 4:
				printf("%s\n", "Gyros offset tag found. Not yet implemented!");
				break;
			case 5:
				printf("%s\n", "Euler angle tag found. Not yet implemented!");
				break;
			case 6:
				printf("%s\n", "References tag found. Not yet implemented!");
				break;
			case 7:
				printf("%s\n", "Trim tag found. Not yet implemented!");
				break;
			case 8:
				printf("%s\n", "RC references tag found. Not yet implemented!");
				break;
			case 9:
				printf("%s\n", "PWM tag found. Not yet implemented!");
				break;
			case 10:
				printf("%s\n", "Altitude tag found. Not yet implemented!");
				break;
			case 11:
				printf("%s\n", "Vision raw tag found. Not yet implemented!");
				break;
			case 12:
				printf("%s\n", "Vision of tag found. Not yet implemented!");
				break;
			case 13:
				printf("%s\n", "Vision tag found. Not yet implemented!");
				break;
			case 14:
				printf("%s\n", "Vision perf tag found. Not yet implemented!");
				break;
			case 15:
				printf("%s\n", "Trackers send tag found. Not yet implemented!");
				break;
			case 16:
				printf("%s\n", "Vision detect tag found. Not yet implemented!");
				break;
			case 17:
				printf("%s\n", "Watchdog tag found. Not yet implemented!");
				break;
			case 18:
				printf("%s\n", "ADC data frame tag found. Not yet implemented!");
				break;
			case 19:
				printf("%s\n", "Video stream tag found. Not yet implemented!");
				break;
			case 20:
				printf("%s\n", "Games tag found. Not yet implemented!");
				break;
			default:
				printf("%s:%d\n", "Unknown option found. ID tag:", optionID);
				this->navDataReadPointer -= optionSize;
				break;
			}
		this->navDataReadPointer += optionSize;
	}

}


/*
 * Decodes Option Demo Tag. Sets navigation data
 */
void ARDroneV1::optionDemoTag(int16 optionSize) {
	this->navData->setControlState(this->readNext4Bytes());
	this->navData->setBattery(this->readNext4Bytes());

	// Set Orientation
	int32 pitch = this->readNext4Bytes();
	int32 roll = this->readNext4Bytes();
	int32 yaw = this->readNext4Bytes();
	int32 height = this->readNext4Bytes();
	this->navData->setPitch(pitch);
	this->navData->setRoll(roll);
	this->navData->setYaw(yaw);
	this->navData->setHeight(height);

	// Set Velocity
	int32 x = this->readNext4Bytes();
	int32 y = this->readNext4Bytes();
	int32 z = this->readNext4Bytes();
	this->navData->setVelocityX(x);
	this->navData->setVelocityY(y);
	this->navData->setVelocityZ(z);

	int32 frameCount = this->readNext4Bytes();
	this->navData->setFrameCount(frameCount);

	// get rid of last bits
	int32 i = 10;
	while(i < optionSize / 4) {
		this->readNext4Bytes();
		++i;
	}

}

/*
 * Check Drone State and set Drone to send full Information if necessary
 */
void ARDroneV1::checkDroneState() {
	int32 state = this->navData->getDroneState();
	if(( state & 0x00000800) > 0) {
		printf("Drone State Check True \n");
		this->navData_droneStateConfig = true;
	}
}

/*
 * Reads next 2 bytes from Navigation Data Buffer
 */
int16 ARDroneV1::readNext2Bytes() {
	int16 retVal = 0;
	for(int i = 0; i < 2; i++) {
		retVal += (*(this->navDataReadPointer) & 0x000000FF) << (i * 8);
		this->navDataReadPointer += 1;
	}
	return retVal;
}

/*
 * Reads next 4 bytes from Navigation Data Buffer
 */
int32 ARDroneV1::readNext4Bytes() {
	int32 retVal = 0;
	for(int i = 0; i < 4; i++) {
		retVal += (*(this->navDataReadPointer) & 0x000000FF) << (i * 8);
		this->navDataReadPointer += 1;
	}
	return retVal;
}


/**
 * Destructor
 */
ARDroneV1::~ARDroneV1() {
	// TODO
	free(this->commandBuffer);
	free(this->navDataBuffer);
	delete this->commandThread;
	delete this->navDataThread;
	delete this->navData;
}

